module NewIterSSolver_m
use SMatrix_m
use CSRMatrix_m
implicit none
type newIterSSolver
    logical :: isSym
    integer :: maxIteration
    real*8 :: IterError
    type(CSRMatrix) :: mat1, mat2
    real*8, allocatable :: diag(:)
contains
    procedure :: initIterSolver => initNewIterSSolver
    procedure :: CG
    procedure :: deallocateNewIterSSolver
end type

contains
subroutine initNewIterSSolver(this, A, maxIter, IEr)
    implicit none
    class(newIterSSolver), intent(inout) :: this
    type(SMatrix), intent(inout) :: A
    integer, intent(in), optional :: maxIter
    real*8, intent(in), optional :: IEr
    
    !call this%mat%initCSRMatrix(A)
    allocate(this.diag(A%M))
    call SMatrixToCSRMatrix(A, this.diag, this.mat1, this.mat2)

    if (present(IEr)) then
        this%maxIteration = maxIter
        this%IterError = IEr
    else if (present(maxIter)) then
        this%maxIteration = maxIter
        this%IterError = epsilon(0.0D0)
    else
        this%maxIteration = 100000
        this%IterError = epsilon(0.0D0)
    end if 
    !write(*, *) this.diag
end subroutine

subroutine CG(this, b, x0, verbosity)
    implicit none
    class(NewIterSSolver), intent(in), target :: this
    real*8, intent(in):: b(:)
    real*8, intent(inout):: x0(:)
    integer, intent(in):: verbosity

    type(CSRMatrix), pointer :: mat
    real*8, allocatable:: Ap(:), p(:), r(:)
    integer :: n, i
    real*8 :: r2
    real*8 :: temp1, alpha, beta
    real*8 :: time_begin, time_end
    
    CALL CPU_TIME (time_begin)
    mat => this%mat1
    n = this%mat1%n
    allocate(Ap(n), p(n), r(n))
    !call this.mat1.mulVectorS(x0, Ap)
    Ap = mat * x0
    !write(*, *) Ap
    
    r = b - Ap
!	r = b - mat * x0
    p = r
    r2 = DOT_PRODUCT(r, r)
    do i = 1, this%maxIteration
        if (sqrt(r2) < this%IterError) then
            exit
        end if
        call mat.mulVectorS(p, Ap)
!        Ap = mat * p
        temp1 = DOT_PRODUCT(Ap, p)
        alpha = r2 / temp1
        x0 = x0 + alpha * p
        r = r - alpha * Ap
        temp1 = DOT_PRODUCT(r, r)
        beta = temp1 / r2
        r2 = temp1
        p = r + beta * p
        if (verbosity >= 2) then
            write(*, *) ""
            write(*, *) "Iteration ", i
            write(*, *) "min", minval(x0), "max", maxval(x0)
            write(*, *) "Error", sqrt(r2)
            write(*, *) ""
        end if
    end do
    deallocate(Ap, p, r)
    CALL CPU_TIME (time_end)
    if (verbosity >= 1) then
        write(*, *) ""
        write(*, *) "Total iteration ", i - 1
        write(*, *) "min", minval(x0), "max", maxval(x0)
        write(*, *) 'Solve time', time_end - time_begin, 'seconds'
        write(*, *) ""
    end if
end subroutine

subroutine deallocateNewIterSSolver(this)
    implicit none
    class(NewIterSSolver), intent(out) :: this
    call this%mat1%deallocateCSRMatrix()
    call this%mat2%deallocateCSRMatrix()

end subroutine


end module    